/// File: Robotics4.NET\BodyMap\BodyMap.cs
/// 
/// ------------------------------------------------------------
/// Copyright (c) 2004
///   Antonio Cisternino (cisterni.unipi.it),
///   Diego Colombo      (colombod.unipi.it)
/// 
/// The use and distribution terms for this software are 
/// contained in the file named license.txt, which can be found 
/// in the root of this distribution.
/// By using this software in any fashion, you are agreeing to 
/// be bound by the terms of this license.
///
/// You must not remove this notice, or any other, from this
/// software.
/// ------------------------------------------------------------

using System;
using System.Collections;
#if WIN
using System.Collections.Generic;
using R4NXMLDocumentContainer = System.Collections.Generic.Dictionary<string, System.Xml.XmlDocument>;
using R4NStringList = System.Collections.Generic.List<string>;
using R4NStickletBrokerContainer = System.Collections.Generic.Dictionary<string, System.Collections.Generic.List<string>>;
using R4NStringListContainer = System.Collections.Generic.Dictionary<string, System.Collections.Generic.List<string>>;
using R4NStringSet = System.Collections.Generic.Dictionary<string, bool>;
using R4NStickletBrokerList = System.Collections.Generic.Dictionary<string, Robotics4.NET.StickletBrokerMessage>;
using R4NInterfaceListContainer = System.Collections.Generic.Dictionary<string, System.Collections.Generic.List<Robotics4.NET.InterfaceEntry>>;
using R4NInterfaceList = System.Collections.Generic.List<Robotics4.NET.InterfaceEntry>;
#endif
using System.IO;
using System.Xml;
using System.Xml.Serialization;
using System.Net;
using System.Net.Sockets;

namespace Robotics4.NET
{

    public delegate void StickletAvailableHandler(StickletBrokerMessage m);

    public class XmlDataReceiver
    {
        string xmldata;
        byte[] data;
        int pos, balance;
        UdpClient incoming;
        IPEndPoint ep;

        public XmlDataReceiver(IPEndPoint e, UdpClient udp)
        {
            incoming = udp;
            xmldata = null;
            data = null;
            pos = -1;
            balance = 0;
            incoming = udp;
            ep = e;
        }

        private int findSplit(byte[] data, int pos, out int openclose)
        {
            int state = 0, ret = 0;
            openclose = 0;
            while (true)
            {
                if (pos == data.Length)
                    return -1;
                switch (state)
                {
                    case 0: // Cerco <
                        switch (data[pos])
                        {
                            case (byte)'<':
                                state = 1;
                                ret = pos++;
                                break;
                            case (byte)'/':
                                state = 16;
                                pos++;
                                break;
                            default:
                                pos++;
                                break;
                        }
                        break;
                    case 16: // Check for self closing tags
                        if (data[pos] == (byte)'>')
                        {
                            openclose--;
                            pos++;
                        }
                        state = 0;
                        break;
                    case 1: // ! o ?
                        switch (data[pos])
                        {
                            case (byte)'!':
                                state = 2;
                                pos++;
                                break;
                            case (byte)'?':
                                state = 12;
                                pos++;
                                break;
                            case (byte)'/':
                                openclose--;
                                pos++;
                                state = 0;
                                break;
                            default:
                                openclose++;
                                state = 0;
                                break;
                        }
                        break;
                    case 2: // [
                        if (data[pos] == (byte)'[')
                        {
                            state = 3;
                            pos++;
                        }
                        else
                            state = 0;
                        break;
                    case 3: // C
                        if (data[pos] == (byte)'C')
                        {
                            state = 4;
                            pos++;
                        }
                        else
                            state = 0;
                        break;
                    case 4: // D
                        if (data[pos] == (byte)'D')
                        {
                            state = 5;
                            pos++;
                        }
                        else
                            state = 0;
                        break;
                    case 5: // A
                        if (data[pos] == (byte)'A')
                        {
                            state = 6;
                            pos++;
                        }
                        else
                            state = 0;
                        break;
                    case 6: // T
                        if (data[pos] == (byte)'T')
                        {
                            state = 7;
                            pos++;
                        }
                        else
                            state = 0;
                        break;
                    case 7: // A
                        if (data[pos] == (byte)'A')
                        {
                            state = 8;
                            pos++;
                        }
                        else
                            state = 0;
                        break;
                    case 8: // [
                        if (data[pos] == (byte)'[')
                        {
                            state = 9;
                            pos++;
                        }
                        else
                            state = 0;
                        break;
                    case 9: // Look for ]
                        if (data[pos] == (byte)']') state = 10;
                        pos++;
                        break;
                    case 10:
                        if (data[pos] == (byte)']')
                        {
                            state = 11;
                            pos++;
                        }
                        else
                            state = 9;
                        break;
                    case 11:
                        if (data[pos] == (byte)'>')
                        {
                            state = 0;
                            pos++;
                        }
                        else
                            state = 10;
                        break;
                    case 12: // ? found
                        if (data[pos] == (byte)'x') state = 13;
                        pos++;
                        break;
                    case 13: // m
                        if (data[pos] == (byte)'m')
                        {
                            state = 14;
                            pos++;
                        }
                        else
                            state = 0;
                        break;
                    case 14: // l
                        if (data[pos] == (byte)'l')
                        {
                            state = 15;
                            pos++;
                        }
                        else
                            state = 0;
                        break;
                    case 15: // blank or ?
                        switch (data[pos])
                        {
                            case (byte)' ':
                            case (byte)'\r':
                            case (byte)'\n':
                            case (byte)'\t':
                            case (byte)'?':
                                return ret;
                            default:
                                state = 0;
                                break;
                        }
                        break;
                }
            }
        }

        /// <summary>
        ///
        /// </summary>
        /// <returns></returns>
        public string ReceiveMessage()
        {
            while (true)
            {
                xmldata = null;
                if (pos == -1)
                {
                    int b;
                    data = incoming.Receive(ref ep);
                    pos = findSplit(data, 0, out b);
                    if (balance == 0 && pos == -1)
                        continue;
                    if (balance != 0 && pos != 0)
                        xmldata += System.Text.Encoding.UTF8.GetString(data, 0, pos == -1 ? data.Length : pos);
                    else
                    {
                        balance = 0;
                        xmldata = null;
                    }
                }
                int end = findSplit(data, pos + 1, out balance);
                if (balance != 0 && end == -1) // Not full message, wait for the rest
                {
                    xmldata = System.Text.Encoding.UTF8.GetString(data, pos, data.Length - pos);
                    pos = -1;
                    continue;
                }
                else // Invalid message: XML parser will justly complain
                    balance = 0;
                if (xmldata == null)
                    xmldata = System.Text.Encoding.UTF8.GetString(data, pos, (end == -1 ? data.Length : end) - pos);
                pos = end;
                return xmldata;
            }
        }
    }

    /// <summary>
    /// This class defines the body map of the robot. All perceptions sent by
    /// peripheral roblets are sent to this roblet (apart from direct messages
    /// required to implement full reactive behavior).
    /// This is an abstract class: KB inherits from this class and implement
    /// the OnSignal method to handle incoming messages.
    /// </summary>
    public abstract class BodyMap
    {
        /// <summary>
        /// Default UDP port used to listen for messages.
        /// </summary>
        private const int defaultPort = 6666;
        /// <summary>
        /// This map contains last messages received by each roblet.
        /// </summary>
        protected R4NXMLDocumentContainer RobletsR;
        /// <summary>
        /// This map contains the interfaces sent by roblets. The interface
        /// condains message examples for input and output, and friend
        /// information.
        /// </summary>
        protected R4NXMLDocumentContainer RobletsW;
        /// <summary>
        /// Direct map of roblet friends. Each roblet can have one or more
        /// roblets as friends. Friends are used to implement full reactivity:
        /// friend roblets may send messages directly, bypassing BodyMap.
        /// BodyMap provides a centralized mechanism to roblets in order
        /// to find their friends.
        /// BEWARE: At this moment only the sender is notified about communication
        /// information of a friend.
        /// </summary>
        protected R4NStringListContainer FriendsMap;
        /// <summary>
        /// Reverse map of friends. It is used to make the information exchange
        /// protocol robust.
        /// </summary>
        protected R4NStringListContainer InverseFriendsMap;
        /// <summary>
        /// List of avaliable sticklets at current time. The list is a dictionary
        /// indexed by the name of the StickletBroker.
        /// </summary>
        protected R4NStickletBrokerList AvaliableSticklets;
        /// <summary>
        /// Set of sticklets that have been activated.
        /// </summary>
        protected R4NStringSet ActiveSticklets;

        /// <summary>
        /// Actual UDP port used by BodyMap to listen for incoming messages.
        /// </summary>
        private int port;

        /// <summary>
        /// Return the default port of the Body Map.
        /// </summary>
        public static int DefaultPort
        {
            get { return defaultPort; }
        }

		protected StickletMessage StickletRequests = new StickletMessage();
        
        /// <summary>
        /// Get or Sets the UDP port used by the BodyMap roblet to listen for 
        /// messages.
        /// </summary>
        public int Port
        {
            get { return port; }
            set { port = value; }
        }

		protected MemoryStream MemStream = new MemoryStream(4096);

		protected static XMLSerializerPool SerializerPool = new XMLSerializerPool();

        /// <summary>
        /// Build a body map roblet.
        /// </summary>
        public BodyMap()
        {
            port = defaultPort;
            RobletsR = new R4NXMLDocumentContainer();
            RobletsW = new R4NXMLDocumentContainer();
            FriendsMap = new R4NStringListContainer();
            InverseFriendsMap = new R4NStringListContainer();
            AvaliableSticklets = new R4NStickletBrokerList();
		}

        /// <summary>
        /// This method implements the listener loop. Messages required to the 
        /// internal protocol are handled internally. The remaining are delivered
        /// to the OnSignal for further processing.
        /// </summary>
        public void Run()
        {
            XmlDataReceiver rcv = new XmlDataReceiver(new IPEndPoint(IPAddress.Any, port), new UdpClient(port));

            while (true)
            {
                string xmldata = rcv.ReceiveMessage();

                XmlDocument doc = new XmlDocument();
                doc.LoadXml(xmldata);
                string robletName = doc.ChildNodes[1].ChildNodes[0].InnerText;

                // Interface sent by some roblet
                if (doc.ChildNodes[1].Name == "RobletInterface")
                {
                    if (doc.ChildNodes[1].Attributes["InterfaceOnBodyMap"] != null)
                    {
                        // FIXME: here we assume interfaces are in the directory of R4N.dll
                        string fn = doc.ChildNodes[1].Attributes["InterfaceOnBodyMap"].Value;
                        
                        string RobletID = doc.SelectSingleNode("/*/RobletID").InnerText;
                        string Name = doc.SelectSingleNode("/*/Name").InnerText;
                        string BrokerHost = doc.SelectSingleNode("/*/BrokerHost").InnerText;
                        string BrokerPort = doc.SelectSingleNode("/*/BrokerPort").InnerText;

                        string msg = File.ReadAllText((new FileInfo(typeof(Robotics4.NET.BodyMap).Assembly.Location)).Directory.FullName + "\\" + fn);
                        msg = msg.Replace("<<HOST>>", BrokerHost);
                        msg = msg.Replace("<<PORT>>", BrokerPort);
                        msg = msg.Replace("<<ID>>", RobletID);
                        msg = msg.Replace("<<NAME>>", Name);

                        doc = new XmlDocument();
                        doc.LoadXml(msg);
                    }

                    // Stores the interface into the appropriate map
                    RobletsW[robletName] = doc;

                    FriendsMap[robletName] = new R4NStringList(); // FIXME: this can be improved
                    if (doc.SelectNodes("/*/IsSticklet")[0].InnerText == "true")
                    {
                        if (!ActiveSticklets.ContainsKey(robletName))
                        {
                            ActiveSticklets[robletName] = true;
                            SendStickletListRequest(doc);
                        }
                    }

                    // Read friends
                    foreach (XmlNode n in doc.SelectNodes("/*/Friends/*"))
                    {
                        string f = n.InnerText;

                        (FriendsMap[robletName]).Add(f);
                        if (!InverseFriendsMap.ContainsKey(f))
                            InverseFriendsMap[f] = new R4NStringList();
                        if (!(InverseFriendsMap[f]).Contains(robletName))
                            (InverseFriendsMap[f]).Add(robletName);
                        if (RobletsW.ContainsKey(f)) // Already there!
                            SendFriendInfo(robletName, f);
                        // Inform friends of the incoming roblet
                        //SendFriendInfo(f, robletName); // Do we want this?
                    }
                    if (InverseFriendsMap.ContainsKey(robletName))

                        foreach (string f in InverseFriendsMap[robletName])
                        {
                            // Inform roblets registered as friends of the one sending the interface
                            SendFriendInfo(f, robletName);
                        }

                    // Other message: store it into the message map.
                }
                else if (doc.ChildNodes[1].Name == "StickletBrokerMessage")
                {
                    if (SerializerPool[typeof(StickletBrokerMessage), ""] == null)
                        SerializerPool[typeof(StickletBrokerMessage), ""] = new XmlSerializer(typeof(StickletBrokerMessage));
                    StickletBrokerMessage msg = SerializerPool[typeof(StickletBrokerMessage), ""].Deserialize(new XmlNodeReader(doc.ChildNodes[1])) as StickletBrokerMessage;
                    AvaliableSticklets.Add(msg.Name, msg);                            
                    OnStickletsAvailable(msg);
                }
                else if (doc.ChildNodes[1].Name == "StickletMessage")
                {
                    if (SerializerPool[typeof(StickletMessage), ""] == null)
                        SerializerPool[typeof(StickletMessage), ""] = new XmlSerializer(typeof(StickletMessage));
                    StickletMessage msg = SerializerPool[typeof(StickletMessage), ""].Deserialize(new XmlNodeReader(doc.ChildNodes[1])) as StickletMessage;
                    if (msg.RequestType == StickletRequest.Detached)
                    {
                        RobletsW.Remove(msg.Name);
                        RobletsR.Remove(msg.Name);
                        ActiveSticklets.Remove(msg.Name);
                    }
                }
                else
                {
                    RobletsR[robletName] = doc;
                    // FIXME: Can a roblet change interface when restarted?
                    if (!RobletsW.ContainsKey(robletName))
                        // Uh-oh, unknown roblet!
                        SendMetadataRequest(doc);

                    // Notify the roblet
                    OnSignal(doc);
                }
            }
        }

        /// <summary>
        /// This method is invoked when perceptions arrive from roblets.
        /// </summary>
        /// <param name="doc">The received XML message</param>
        protected abstract void OnSignal(XmlDocument doc);

        /// <summary>
        /// Occurs when a new StickletBroker joins the bodymap (delegate_sgruvio_mode).
        /// </summary>
        public event StickletAvailableHandler StickletsAvailable;

        /// <summary>
        /// Occurs when a new StickletBroker joins the bodymap (virtual_sgruvio_mode).
        /// </summary>
        /// <param name="m">Descriptor of the new StickletBroker</param>
        protected virtual void OnStickletsAvailable(StickletBrokerMessage m) {
            if (StickletsAvailable != null)
                StickletsAvailable(m);
        }

        /// <summary>
        /// Ask a roblet for its interface.
        /// </summary>
        /// <param name="doc">Message containing information about how to contace 
        /// the roblet</param>
        private void SendMetadataRequest(XmlDocument doc)
        {
            MetaDataRequest r = new MetaDataRequest();
            r.RobletID = int.Parse(doc.SelectSingleNode("/*/RobletID").InnerText);
            r.Name = "BodyMap";
            r.BrokerHost = doc.SelectSingleNode("/*/BrokerHost").InnerText;
            r.BrokerPort = int.Parse(doc.SelectSingleNode("/*/BrokerPort").InnerText);
            SendState(r);
        }

        /// <summary>
        /// Ask a StickletBroker for the sticklet list.
        /// </summary>
        /// <param name="doc">Message containing information about how to contact 
        /// the broker</param>
        private void SendStickletListRequest(XmlDocument doc)
        {
            StickletMessage r = new StickletMessage();
            r.RequestType = StickletRequest.StickletList;
            r.RobletID = int.Parse(doc.SelectSingleNode("/*/RobletID").InnerText);
            r.Name = "BodyMap";
            r.BrokerHost = doc.SelectSingleNode("/*/BrokerHost").InnerText;
            r.BrokerPort = int.Parse(doc.SelectSingleNode("/*/BrokerPort").InnerText);
            SendState(r);
        }

        /// <summary>
        /// Send information about friendship to a given roblet.
        /// </summary>
        /// <param name="to">Roblet name that should receive the contact infos</param>
        /// <param name="f">Roblet name whose infos should be sent</param>
        private void SendFriendInfo(string to, string f)
        {
            if (!RobletsW.ContainsKey(to)) return;

            XmlDocument doc = RobletsW[f];
            XmlDocument d = RobletsW[to];

            FriendInfo msg = new FriendInfo();
            msg.FriendRobletName = f;
            msg.FriendRobletID = int.Parse(doc.SelectSingleNode("/*/RobletID").InnerText);
            msg.FriendBrokerHost = doc.SelectSingleNode("/*/BrokerHost").InnerText;
            msg.FriendBrokerPort = int.Parse(doc.SelectSingleNode("/*/BrokerPort").InnerText);
            msg.RobletID = int.Parse(d.SelectSingleNode("/*/RobletID").InnerText);
            msg.BrokerHost = d.SelectSingleNode("/*/BrokerHost").InnerText;
            msg.BrokerPort = int.Parse(d.SelectSingleNode("/*/BrokerPort").InnerText);
            SendState(msg);
        }

        protected void AttachSticklet(StickletBrokerMessage msg, string name)
        {
            StickletRequests.RequestType = StickletRequest.Attach;
			StickletRequests.StickletName = name;
            msg.CopyDeliveryInfo(StickletRequests);
            SendState(StickletRequests);
        }

        protected void DetachSticklet(string name)
        {
			StickletRequests.RequestType = StickletRequest.Detach;
			StickletRequests.StickletName = name;
            XmlDocument doc = RobletsW[name];
            StickletRequests.RequestType = StickletRequest.StickletList;
            StickletRequests.RobletID = int.Parse(doc.SelectSingleNode("/*/RobletID").InnerText);
            StickletRequests.Name = "BodyMap";
            StickletRequests.BrokerHost = doc.SelectSingleNode("/*/BrokerHost").InnerText;
            StickletRequests.BrokerPort = int.Parse(doc.SelectSingleNode("/*/BrokerPort").InnerText);
            SendState(StickletRequests);
        }

        /// <summary>
        /// Send a roblet message to the appropriate broker.
        /// </summary>
        /// <param name="msg">Message to be sent.</param>
        protected void SendState(RobletMessage msg)
        {
			if (SerializerPool[msg.GetType(), msg.Name] == null)
				SerializerPool[msg.GetType(), msg.Name] = new XmlSerializer(msg.GetType());

			SerializerPool[msg.GetType(), msg.Name].Serialize(MemStream, msg);
			byte[] data = MemStream.GetBuffer();
			(new UdpClient(msg.BrokerHost, msg.BrokerPort)).Send(data, (int)MemStream.Length);
			MemStream.Position = 0;
			MemStream.SetLength(0);
        }

        /// <summary>
        /// Send a roblet message to the appropriate broker.
        /// </summary>
        /// <param name="msg">Message to be sent.</param>
        /// <param name="ctxt"></param>
        protected void SendState(RobletMessage msg, Type[] ctxt)
        {
			if (SerializerPool[msg.GetType(), msg.Name] == null)
				SerializerPool[msg.GetType(), msg.Name] = new XmlSerializer(msg.GetType());
			
            XmlTextWriter xt = new XmlTextWriter(MemStream, System.Text.Encoding.ASCII);
			SerializerPool[msg.GetType(), msg.Name].Serialize(xt, msg);

            byte[] data = MemStream.GetBuffer();
            (new UdpClient(msg.BrokerHost, msg.BrokerPort)).Send(data, (int)MemStream.Length);
			MemStream.Position = 0;
			MemStream.SetLength(0);
        }

        /// <summary>
        /// Send a roblet message to the appropriate broker.
        /// </summary>
        /// <param name="d">Message to be sent.</param>
        protected void SendState(XmlNode d)
        {
            byte[] data = System.Text.Encoding.UTF8.GetBytes("<?xml version=\"1.0\" encoding=\"utf-8\" ?>" + d.OuterXml);
            (new UdpClient(d.SelectSingleNode("/BrokerHost").InnerText, int.Parse(d.SelectSingleNode("/BrokerPort").InnerText))).Send(data, data.Length);
        }

        /// <summary>
        /// Send a roblet message to the appropriate broker.
        /// </summary>
        /// <param name="doc">Message to be sent.</param>
        protected void SendState(XmlDocument doc)
        {
            doc.WriteContentTo(new XmlTextWriter(new StreamWriter(MemStream, System.Text.Encoding.UTF8)));
            byte[] data = MemStream.GetBuffer();
            (new UdpClient(doc.SelectSingleNode("/*/BrokerHost").InnerText, int.Parse(doc.SelectSingleNode("/*/BrokerPort").InnerText))).Send(data, (int)MemStream.Length);
			MemStream.Position = 0;
			MemStream.SetLength(0);
        }
    }

#if WIN
    /// <summary>
    /// This class is used for debugging purposes.
    /// </summary>
    public class BodyMapDebug : BodyMap
    {
        /// <summary>
        /// This method simply prints the received message.
        /// </summary>
        /// <param name="doc">Message dispatched by body map.</param>
        protected override void OnSignal(XmlDocument doc)
        {
            doc.WriteTo(new XmlTextWriter(Console.Out));
            Console.WriteLine();
        }
    }
#endif
}
